#include "Matrix.h"

#include <cmath>
#include <climits>
#include <cfloat>
#include <cassert>

#define SIGN(a) ((a) >= 0 ? 1 : -1)
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) > (b) ? (b) : (a))
#define ACCURACY 1e-12
// 精度不要取太高，否则在隐式 QR 迭代中的 householder 变换可能需要变换一个首元素非常小，其余元素为零的向量
// 这可能导致很大的误差，得到不好的结果
#define ITERATION 1000

using namespace LinearAlgebra;

// 模板显式实例化，从而实现模板与实现的分离
template class DynamicArray<int>;
template class DynamicArray<Real>;

/**
 * @file Matrix.cpp
 * @author xingyifan
 * @date 2022-11-29 11:45
 *
 * @description: 成员函数定义
 */

// 初始化构造函数
Vector::Vector(int D, Real t) : m_dim(D)
{
    m_value = new Real[D];
    for (int i = 0; i < D; m_value[i++] = t)
        ;
}

// 可以通过列表初始化
Vector::Vector(std::initializer_list<Real> l) : m_dim(l.size())
{
    m_value = new Real[m_dim];
    auto j = l.begin();
    for (int d = 0; d < m_dim; ++d)
        m_value[d] = *j++;
}

// 拷贝构造
Vector::Vector(const Vector &rhs) : m_dim(rhs.size())
{
    m_value = new Real[m_dim];
    for (int d = 0; d < m_dim; d++)
        m_value[d] = rhs.m_value[d];
}

// 获得向量长度
int Vector::size() const
{
    return m_dim;
}

// 返回引用
Real &Vector::operator[](int i)
{
    assert(i < m_dim && i >= 0);
    return m_value[i];
}

// 返回值
Real Vector::operator[](int i) const
{
    assert(i < m_dim && i >= 0);
    return m_value[i];
}

// 向量加减
Vector Vector::operator+(const Vector &rhs) const
{
    Vector res = *this;
    for (int i = 0; i < m_dim; i++)
        res[i] += rhs[i];
    return res;
}
Vector Vector::operator-(const Vector &rhs) const
{
    Vector res = *this;
    for (int i = 0; i < m_dim; i++)
        res[i] -= rhs[i];
    return res;
}

// 常数乘除法
Vector Vector::operator*(Real t) const
{
    Vector res = *this;
    for (int d = 0; d < m_dim; ++d)
        res[d] *= t;
    return res;
}
Vector Vector::operator/(Real t) const
{
    assert(fabs(t) > ACCURACY);
    Vector res = *this;
    for (int d = 0; d < m_dim; ++d)
        res[d] /= t;
    return res;
}

// 向量乘矩阵
Matrix Vector::operator*(const Matrix &rhs) const
{
    assert(rhs.row() == 1);
    Matrix res(m_dim, rhs.col());
    for (int i = 0; i < res.row(); ++i)
        for (int j = 0; j < res.col(); ++j)
            res(i, j) = m_value[i] * rhs(0, j);
    return res;
}

// 赋值
void Vector::operator=(const Vector &rhs)
{
    assert(m_dim == rhs.m_dim);
    for (int d = 0; d < m_dim; d++)
        m_value[d] = rhs[d];
}

// 取反操作
Vector Vector::operator-() const
{
    Vector res = *this;
    for (int d = 0; d < m_dim; ++d)
        res[d] *= -1;
    return res;
}

// 冒泡排序
void Vector::sort()
{
    for (int i = 0; i < m_dim; i++)
    {
        for (int j = 1; j < m_dim; j++)
        {
            // 比较相邻的元素，如果后者小于前者就交换
            if (m_value[j] < m_value[j - 1])
            {
                Real tmp = m_value[j];
                m_value[j] = m_value[j - 1];
                m_value[j - 1] = tmp;
            }
        }
    }
}

// 反转向量
Vector Vector::reverse()
{
    Vector res(m_dim);
    for (int i = 0; i < m_dim; i++)
        res[i] = m_value[m_dim - i - 1];
    return res;
}

// 从 i 开始查找元素，返回第一个下标
int Vector::find(Real t, int i) const
{
    for (int j = i; j < m_dim; j++)
        if (fabs(m_value[j] - t) < ACCURACY)
            return j;
    return i - 1;
}

// 元素求和
Real Vector::sum() const
{
    Real a = 0;
    for (int i = 0; i < m_dim; i++)
        a += m_value[i];
    return a;
}

// 取绝对值
Vector Vector::abs() const
{
    Vector res(m_dim);
    for (int d = 0; d < m_dim; ++d)
        res[d] = fabs(m_value[d]);
    return res;
}

// 获取符号向量
Vector Vector::sgn() const
{
    Vector res(m_dim);
    for (int d = 0; d < m_dim; ++d)
        res[d] = (m_value[d] > 0 ? 1 : (m_value[d] < 0 ? -1 : 0));
    return res;
}

// 计算范数
Real Vector::norm(int nt) const
{
    if (nt == 2)
    {
        return sqrt(dot(*this, *this));
    }
    else if (nt == 1)
    {
        Real a = 0;
        for (int i = 0; i < m_dim; ++i)
            a += fabs(m_value[i]);
        return a;
    }
    // 无穷范数
    else if (nt == 0)
    {
        Real a = fabs(m_value[0]);
        for (int i = 1; i < m_dim; i++)
        {
            a = MAX(fabs(m_value[i]), a);
        }
        return a;
    }
    return 0;
}

// 取最小元
Real Vector::Min() const
{
    Real a = m_value[0];
    for (int d = 1; d < m_dim; d++)
        if (m_value[d] < a)
            a = m_value[d];
    return a;
}

// 取最大元
Real Vector::Max() const
{
    Real a = m_value[0];
    for (int d = 1; d < m_dim; d++)
        if (m_value[d] > a)
            a = m_value[d];
    return a;
}

// 取最大模的分量
int Vector::maxIndex() const
{
    int index = 0;
    for (int d = 1; d < m_dim; d++)
        index = (fabs(m_value[d]) > fabs(m_value[index]) ? d : index);
    return index;
}

// 转化成矩阵
Matrix Vector::toMatrix() const
{
    Matrix m(m_dim, 1);
    for (int i = 0; i < m_dim; i++)
        m(i, 0) = m_value[i];
    return m;
}

// 转置
Matrix Vector::transpose() const
{
    Matrix m(1, m_dim);
    for (int i = 0; i < m_dim; i++)
        m(0, i) = m_value[i];
    return m;
}

// 释放资源
Vector::~Vector()
{
    delete[] m_value;
}

// 初始化构造函数
Matrix::Matrix(int Row, int Col, Real t) : m_row(Row), m_col(Col)
{
    m_value = new Real *[Row];
    for (int i = 0; i < Row; i++)
    {
        m_value[i] = new Real[Col];
        for (int j = 0; j < Col; j++)
            m_value[i][j] = t;
    }
}

// 通过列表初始化
Matrix::Matrix(int Row, int Col, std::initializer_list<Real> l) : m_row(Row), m_col(Col)
{
    assert(l.size() == Row * Col);
    m_value = new Real *[Row];
    auto it = l.begin();
    for (int i = 0; i < Row; i++)
    {
        m_value[i] = new Real[Col];
        for (int j = 0; j < Col; j++)
        {
            m_value[i][j] = *it;
            *it++;
        }
    }
}

// 拷贝构造
Matrix::Matrix(const Matrix &rhs) : m_row(rhs.m_row), m_col(rhs.m_col)
{
    m_value = new Real *[m_row];
    for (int i = 0; i < m_row; i++)
    {
        m_value[i] = new Real[m_col];
        for (int j = 0; j < m_col; j++)
            m_value[i][j] = rhs.m_value[i][j];
    }
}

// 获得行数
int Matrix::row() const
{
    return m_row;
}

// 获得列数
int Matrix::col() const
{
    return m_col;
}

// 返回第 j 列
Vector Matrix::operator()(int j) const
{
    assert(j < m_col && j >= 0);
    Vector v(m_row);
    for (int i = 0; i < m_row; i++)
        v[i] = m_value[i][j];
    return v;
}

// 返回 i,j 元素
Real Matrix::operator()(int i, int j) const
{
    assert(i < m_row && j < m_col && i >= 0 && j >= 0);
    return m_value[i][j];
}

// 返回 i,j 元素引用
Real &Matrix::operator()(int i, int j)
{
    assert(i < m_row && j < m_col && i >= 0 && j >= 0);
    return m_value[i][j];
}

// 矩阵加减乘
Matrix Matrix::operator+(const Matrix &rhs) const
{
    assert(m_row == rhs.m_row && m_col == rhs.m_col);
    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) += rhs(i, j);
    return m;
}
Matrix Matrix::operator-(const Matrix &rhs) const
{
    assert(m_row == rhs.m_row && m_col == rhs.m_col);
    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) -= rhs(i, j);
    return m;
}

// 矩阵乘法
Matrix Matrix::operator*(const Matrix &rhs) const
{
    assert(m_col == rhs.m_row);
    int Col2 = rhs.m_col;
    Matrix m(m_row, Col2);
    for (int i = 0; i < m_row; i++)
    {
        for (int j = 0; j < Col2; j++)
        {
            Real sum = 0;
            for (int k = 0; k < m_col; k++)
                sum += m_value[i][k] * rhs(k, j);
            m(i, j) = sum;
        }
    }
    return m;
}
Vector Matrix::operator*(const Vector &v) const
{
    assert(m_col == v.size());
    Vector res(m_row);
    for (int i = 0; i < m_row; i++)
    {
        Real sum = 0;
        for (int j = 0; j < m_col; j++)
            sum += m_value[i][j] * v[j];
        res[i] = sum;
    }
    return res;
}
Matrix Matrix::operator*(Real t) const
{
    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) *= t;
    return m;
}
Matrix Matrix::operator/(Real t) const
{
    assert(fabs(t) > ACCURACY);

    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) /= t;
    return m;
}

// 直接赋值
void Matrix::operator=(const Matrix &rhs)
{
    assert(m_row == rhs.m_row && m_col == rhs.m_col);
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m_value[i][j] = rhs(i, j);
}

// 取反操作
Matrix Matrix::operator-() const
{
    Matrix m = *this;
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) *= -1;
    return m;
}

// 在 j 列插入列向量 v
void Matrix::insert(int j, Vector &v)
{
    assert(m_row == v.size() && j < m_col);
    for (int i = 0; i < m_row; i++)
        m_value[i][j] = v[i];
}

// 计算范数
Real Matrix::norm(int nt) const
{
    const Matrix &lhs = *this;
    if (nt == 1)
    {
        Real max = 0;
        for (int i = 0; i < m_col; i++)
        {
            Real a = 0;
            for (int j = 0; j < m_row; a += fabs(lhs(j++, i)))
                ;
            max = MAX(a, max);
        }
        return max;
    }
    // 无穷范数
    else if (nt == 0)
    {
        Real max = 0;
        for (int i = 0; i < m_row; i++)
        {
            Real a = 0;
            for (int j = 0; j < m_col; a += fabs(lhs(i, j++)))
                ;
            max = MAX(a, max);
        }
        return max;
    }
    return 0;
}

// e 范数 / f 范数
Real Matrix::norm(char c) const
{
    const Matrix &lhs = *this;
    if (c == 'e')
    {
        Real sum = 0;
        for (int i = 0; i < m_row; i++)
            for (int j = 0; j < m_col; j++)
                if (i != j)
                    sum += lhs(i, j) * lhs(i, j);
        return sqrt(sum);
    }
    else if (c == 'f')
    {
        Real sum = 0;
        for (int i = 0; i < m_row; i++)
            for (int j = 0; j < m_col; j++)
                sum += lhs(i, j) * lhs(i, j);
        return sqrt(sum);
    }
    return 0;
}

// 获取上三角
Matrix Matrix::upper() const
{
    const Matrix &A = *this;
    Matrix m(m_row, m_col);
    int limit = MIN(m_row, m_col);
    for (int i = 0; i < limit; i++)
        for (int j = i; j < limit; j++)
            m(i, j) = A(i, j);
    return m;
}

// 获得下三角
Matrix Matrix::lower() const
{
    const Matrix &A = *this;
    Matrix m(m_row, m_col);
    int limit = MIN(m_row, m_col);
    for (int i = 0; i < limit; i++)
        for (int j = 0; j < i + 1; j++)
            m(i, j) = A(i, j);
    return m;
}

// 转置矩阵
Matrix Matrix::transpose() const
{
    const Matrix &A = *this;
    Matrix m(m_col, m_row);
    for (int i = 0; i < m_col; i++)
        for (int j = 0; j < m_row; j++)
            m(i, j) = A(j, i);
    return m;
}

// 矩阵求逆
Matrix Matrix::inverse() const
{
    assert(m_row == m_col);

    // 初始化单位阵
    Matrix m(m_row, m_row);
    for (int i = 0; i < m_row; i++)
        m(i, i) = 1;

    // 保存当前矩阵
    Matrix old = *this;

    Real max;  // 最大值
    int index; // 最大值位置

    for (int i = 0; i < m_row; i++)
    {
        max = 0;
        index = i;

        // 寻找最大列元作为主元
        for (int j = i; j < m_row; j++)
        {
            Real tmp = fabs(old(j, i));
            if (tmp > max)
            {
                max = tmp;
                index = j;
            }
        }

        // 确保主元足够大
        assert(max > ACCURACY);

        // 交换最大元到对角位置
        for (int j = 0; j < m_row; j++)
        {
            Real tmp = old(i, j);
            old(i, j) = old(index, j);
            old(index, j) = tmp;

            tmp = m(i, j);
            m(i, j) = m(index, j);
            m(index, j) = tmp;
        }

        // Gauss 消去
        for (int j = 0; j < m_row; j++)
        {
            Real tmp = old(i, i);
            if (i == j)
            {
                for (int k = 0; k < m_row; k++)
                {
                    m(j, k) /= tmp;
                    old(j, k) /= tmp;
                }
                continue;
            }
            tmp = old(j, i) / old(i, i);
            for (int k = 0; k < m_row; k++)
            {
                m(j, k) -= tmp * m(i, k);
                old(j, k) -= tmp * old(i, k);
            }
        }
    }

    return m;
}

// LU 分解
Matrix Matrix::LU() const
{
    assert(m_row == m_col);
    Matrix A = *this;

    for (int i = 0; i < m_row; i++)
    {
        Real key = A(i, i);

        // 确保对角元足够大
        assert(fabs(key) > ACCURACY);

        // 第 i 列高斯变换
        for (int j = i + 1; j < m_row; j++)
        {
            A(j, i) /= key;
            for (int k = i + 1; k < m_row; k++)
                A(j, k) -= A(i, k) * A(j, i);
        }
    }
    return A;
}

// 列主元 LU 分解，传入下标数组，返回置换后的数组
Matrix Matrix::LU(int perm[]) const
{
    assert(m_row == m_col);
    Matrix A = *this;

    Real max, tmp; // 记录最大值
    int index;     // 记录要交换的主元位置

    for (int i = 0; i < m_row; i++)
    {
        max = 0;
        index = i;

        // 选取列主元
        for (int p = i; p < m_row; p++)
        {
            tmp = fabs(A(p, i));
            if (tmp > max)
            {
                max = tmp;
                index = p;
            }
        }

        // 确保主元足够大
        assert(max > ACCURACY);

        // 交换主元
        for (int q = 0; q < m_row; q++)
        {
            tmp = A(i, q);
            A(i, q) = A(index, q);
            A(index, q) = tmp;
        }

        // 交换指标，记录置换
        int tmp_ind = perm[i];
        perm[i] = perm[index];
        perm[index] = tmp_ind;

        // 正常的高斯消去法
        for (int j = i + 1; j < m_row; j++)
        {
            A(j, i) /= A(i, i);
            for (int k = i + 1; k < m_row; k++)
                A(j, k) -= A(i, k) * A(j, i);
        }
    }

    return A;
}

// Cholesky
Matrix Matrix::Cholesky() const
{
    assert(m_row == m_col);
    Matrix A = *this;

    for (int i = 0; i < m_row; i++)
    {
        Real sum = 0;
        for (int p = 0; p < i; p++)
            sum += A(i, p) * A(i, p);

        A(i, i) = sqrt(A(i, i) - sum);

        // 确保对角元足够大
        assert(fabs(A(i, i) > ACCURACY));

        for (int j = i + 1; j < m_row; j++)
        {
            sum = 0;
            for (int k = 0; k < i; k++)
                sum += A(j, k) * A(i, k);

            A(j, i) = (A(j, i) - sum) / A(i, i);
            A(i, j) = A(j, i);
        }
    }

    return A;
}

// 优化的 Cholesky
Matrix Matrix::CholeskyD() const
{
    assert(m_row == m_col);
    Matrix A = *this;

    for (int i = 0; i < m_row; i++)
    {
        Real sum = 0;
        for (int p = 0; p < i; p++)
            sum += A(i, p) * A(i, p) * A(p, p);

        A(i, i) -= sum;

        // 确保对角元足够大
        assert(fabs(A(i, i) > ACCURACY));

        for (int j = i + 1; j < m_row; j++)
        {
            sum = 0;
            for (int k = 0; k < i; k++)
                sum += A(j, k) * A(i, k) * A(k, k);

            A(j, i) -= sum;
            A(j, i) /= A(i, i);
            A(i, j) = A(j, i);
        }
    }

    return A;
}

// QR 分解
Matrix Matrix::QR(Vector &d) const
{
    assert(d.size() == m_col);
    Matrix A = *this;

    for (int i = 0; i < m_col; i++)
    {
        Vector x(m_row - i);
        Vector v(m_row - i);

        for (int j = i; j < m_row; j++)
            x[j - i] = A(j, i);

        Real beta = Householder(x, v);
        Vector w(m_col - i);

        // get w = beta * AT * v
        for (int k = i; k < m_col; k++)
        {
            Real sum = 0;
            for (int p = i; p < m_row; p++)
                sum += A(p, k) * v[p - i];

            // note: it is w[k-i], not w[k]
            w[k - i] = beta * sum;
        }

        // get HA = A - v * wT
        for (int k = i; k < m_row; k++)
        {
            for (int p = i; p < m_col; p++)
            {
                if (p == i && k > i)
                    A(k, p) = v[k - i];
                else
                    A(k, p) -= v[k - i] * w[p - i];
            }
        }

        d[i] = beta;
    }

    return A;
}

// 上 Hessenberg 化，在 Q 中累计正交变换
Matrix Matrix::Hessenberg(Matrix *Q) const
{
    assert(m_row == m_col);
    Matrix A = *this;

    int n = m_row;
    for (int i = 0; i < n - 2; i++)
    {
        // 用于保存 Householder 变换
        Vector x(n - i - 1);
        Vector v(n - i - 1);

        // x 取 A 的 i 列对角元以下的部分
        for (int j = i + 1; j < n; j++)
            x[j - i - 1] = A(j, i);

        // 为了节约，将 beta 存放在 v 头部
        Real beta = Householder(x, v);
        v[0] = beta;

        // 注意列的范围要从 0 到 n，不然右上角的元素无法更新
        A.HA(i + 1, n, v);
        A.AH(i + 1, n, v);

        // 累计正交变换
        if (Q != nullptr)
            Q->AH(i + 1, n, v);
    }

    return A;
}

// 对称阵化为三对角对称阵，在 Q 中累计正交变换
Matrix Matrix::Tridiagonal(Matrix *Q) const
{
    assert(m_row == m_col);
    Matrix A = *this;

    int n = m_row;
    for (int i = 0; i < n - 2; i++)
    {
        // 用于保存 Householder 变换
        Vector x(n - i - 1);
        Vector v(n - i - 1);

        // x 取 A 的 i 列对角元以下的部分
        for (int j = i + 1; j < n; j++)
            x[j - i - 1] = A(j, i);

        // 为了节约，将 beta 存放在 v 头部
        Real beta = Householder(x, v);
        v[0] = beta;

        // 累计正交变换
        if (Q != nullptr)
            Q->AH(i + 1, n, v);

        // 注意列的范围要从 0 到 n，不然右上角的元素无法更新
        A.HAHs(i + 1, n, v);
    }

    return A;
}

// 矩阵上二对角化，在 U,V 中累计正交变换
Matrix Matrix::Didiagonal(Matrix *U, Matrix *V) const
{
    Matrix A = *this;
    assert(A.row() >= A.col());

    int n = A.col();
    int m = A.row();
    // m >= n
    for (int i = 0; i < n; i++)
    {
        // 用于保存 Householder 变换
        Vector x(m - i);
        Vector v(m - i);

        // x 取 A 的 i 列对角元和以下的部分
        for (int j = i; j < m; j++)
            x[j - i] = A(j, i);

        Real beta = Householder(x, v);
        v[0] = beta;

        // 累计正交变换
        if (U != nullptr)
            U->AH(i, m, v);

        A.HA(i, m, v);

        if (i < n - 2)
        {
            // 用于保存 Householder 变换
            Vector x(n - i - 1);
            Vector v(n - i - 1);

            // x 取 A 的 i 行对角元以下的部分
            for (int j = i + 1; j < n; j++)
                x[j - i - 1] = A(i, j);

            Real beta = Householder(x, v);
            v[0] = beta;

            // 累计正交变换
            if (V != nullptr)
                V->AH(i + 1, n, v);

            A.AH(i + 1, n, v);
        }
    }

    return A;
}

// 计算指定范围的 Householder 变换，作用结果为 HA
void Matrix::HA(int startOfRow, int endOfRow, Vector &v)
{
    Matrix &A = *this;
    int n = A.col();

    // 先获得变换的变量
    Real beta = v[0];
    v[0] = 1;

    // 逐列计算 HA
    for (int j = 0; j < n; j++)
    {
        Real sum = 0;
        // 计算 beta * vT * Aj
        for (int k = startOfRow; k < endOfRow; k++)
            sum += beta * v[k - startOfRow] * A(k, j);

        // 计算 Aj - v * beta * vT * Aj
        for (int k = startOfRow; k < endOfRow; k++)
            A(k, j) -= sum * v[k - startOfRow];
    }

    // 最后还原 v[0]
    v[0] = beta;
}

// 计算指定范围的 Householder 变换，作用结果为 AH
void Matrix::AH(int startOfCol, int endOfCol, Vector &v)
{
    Matrix &A = *this;
    int n = A.row();

    // 先获得变换的变量
    Real beta = v[0];
    v[0] = 1;

    // 逐行计算 AH
    for (int j = 0; j < n; j++)
    {
        Real sum = 0;
        // 计算 beta * ATj * v
        for (int k = startOfCol; k < endOfCol; k++)
            sum += beta * v[k - startOfCol] * A(j, k);

        // 计算 ATj - beta * ATj * v * vT
        for (int k = startOfCol; k < endOfCol; k++)
            A(j, k) -= sum * v[k - startOfCol];
    }

    // 最后还原 v[0]
    v[0] = beta;
}

void Matrix::HAHs(int startOfRow, int endOfRow, Vector &v)
{
    Matrix &A = *this;
    int n = A.row();
    assert(n == A.col());

    // HAH = A - v *wT - w * vT
    Real beta = v[0];
    v[0] = 1;
    Vector u(endOfRow - startOfRow), w(endOfRow - startOfRow);

    // u = beta * A * v
    for (int i = startOfRow; i < endOfRow; i++)
    {
        Real sum = 0;
        for (int j = startOfRow; j < endOfRow; j++)
            sum += A(i, j) * v[j - startOfRow];

        u[i - startOfRow] = beta * sum;
    }

    // w = u - 1/2 * beta * (vT *u) * v
    w = u - v * dot(v, u) * beta * 0.5;

    // householder 变换后，次对角应当变成这一列的模长，同时这一列其它元素归零
    Real sum = 0;
    for (int i = startOfRow; i < endOfRow; i++)
    {
        sum += A(i, startOfRow - 1) * A(i, startOfRow - 1);
        A(i, startOfRow - 1) = A(startOfRow - 1, i) = 0;
    }
    A(startOfRow - 1, startOfRow) = A(startOfRow, startOfRow - 1) = sqrt(sum);

    // 计算 A - vw^T - wv^T
    for (int i = startOfRow; i < endOfRow; i++)
        for (int j = startOfRow; j < endOfRow; j++)
            A(i, j) -= v[i - startOfRow] * w[j - startOfRow] + v[j - startOfRow] * w[i - startOfRow];

    // 最后还原 v[0]
    v[0] = beta;
}

// 计算指定分量的 Givens 变换，得到 GA
void Matrix::GA(int i, int j, Vector &cs)
{
    Matrix &A = *this;
    int n = A.col();

    // 计算 G*A
    for (int k = 0; k < n; k++)
    {
        Real tmp = A(i, k);
        A(i, k) = tmp * cs[0] + A(j, k) * cs[1];
        A(j, k) = -cs[1] * tmp + A(j, k) * cs[0];
    }
}

// 计算指定分量的 Givens 变换，得到 AG
void Matrix::AG(int i, int j, Vector &cs)
{
    Matrix &A = *this;
    int n = A.row();

    for (int k = 0; k < n; k++)
    {
        Real tmp = A(k, i);
        A(k, i) = tmp * cs[0] - A(k, j) * cs[1];
        A(k, j) = cs[1] * tmp + A(k, j) * cs[0];
    }
}

// 计算指定分量的 Givens 变换，得到 AGT
void Matrix::AGT(int i, int j, Vector &cs)
{
    Matrix &A = *this;
    int n = A.row();

    for (int k = 0; k < n; k++)
    {
        Real tmp = A(k, i);
        A(k, i) = tmp * cs[0] + A(k, j) * cs[1];
        A(k, j) = -cs[1] * tmp + A(k, j) * cs[0];
    }
}

// 双重步 QR 迭代
void Matrix::doubleQR(int l, int m, Matrix *Q)
{
    assert(m_row == m_col && l < m_row && m < m_col);

    Matrix &A = *this;
    Real s = A(m_row - 2, m_row - 2) + A(m_row - 1, m_row - 1);
    Real t = A(m_row - 2, m_row - 2) * A(m_row - 1, m_row - 1) - A(m_row - 2, m_row - 1) * A(m_row - 1, m_row - 2);

    Vector x(3);
    x[0] = A(0, 0) * A(0, 0) + A(0, 1) * A(1, 0) - s * A(0, 0) + t;
    x[1] = A(1, 0) * (A(0, 0) + A(1, 1) - s);
    x[2] = A(1, 0) * A(2, 1);
    // 不考虑 n<3 的情况，即 A 至少是 3 阶矩阵

    // 计算 P0 * H * P0
    Vector v(3);
    Real beta = Householder(x, v);
    v[0] = beta;

    // 累计正交变换
    if (Q != nullptr)
        Q->AH(l, l + 3, v);

    // 计算 P0 * H * P0
    HA(l, l + 3, v);
    AH(l, l + 3, v);

    // 开始双重步迭代
    x[0] = A(l + 1, l);
    x[1] = A(l + 2, l);
    for (int i = l + 1; i < m_row - m - 2; i++)
    {
        x[2] = A(i + 2, i - 1);
        beta = Householder(x, v);
        v[0] = beta;

        // 累计正交变换
        if (Q != nullptr)
            Q->AH(i, i + 3, v);

        HA(i, i + 3, v);
        AH(i, i + 3, v);

        x[0] = A(i + 1, i);
        x[1] = A(i + 2, i);
    }

    // 最后一步只有 2 阶
    Vector y({x[0], x[1]});
    Vector v2(2);
    beta = Householder(y, v2);
    v2[0] = beta;

    // 累计正交变换
    if (Q != nullptr)
        Q->AH(m_row - m - 2, m_row - m, v2);

    HA(m_row - m - 2, m_row - m, v2);
    AH(m_row - m - 2, m_row - m, v2);
}

// Wilkinson QR 迭代，针对三对角对称矩阵
void Matrix::WilkinsonQR(int l, int m, Matrix *Q)
{
    assert(m_row == m_col && l < m_row && m < m_col);

    Matrix &T = *this;
    int n = m_row;
    int last = n - m - 1;

    // 计算 Wilkinson 位移
    Real d = (T(last - 1, last - 1) - T(last, last)) / 2;
    Real mu = T(last, last) - T(last, last - 1) * T(last, last - 1) /
                                  (d + SIGN(d) * sqrt(d * d + T(last, last - 1) * T(last, last - 1)));

    Vector xz({T(l, l) - mu, T(l + 1, l)});
    Vector cs(2);
    for (int i = l; i < last; i++)
    {
        // 计算 Givens 变换的系数
        cs = Givens(xz);

        // 计算 G*T
        T.GA(i, i + 1, cs);
        T.AGT(i, i + 1, cs);

        // 累计正交变换 Q * G
        if (Q != nullptr)
            Q->AGT(i, i + 1, cs);

        // 只要不是最后一行，就获取相邻两行
        if (i < last - 1)
        {
            xz[0] = T(i + 1, i);
            xz[1] = T(i + 2, i);
        }
    }
}

// Wilkinson 位移的 SVD 迭代
void Matrix::WilkinsonSVD(int p, int q, Matrix *U, Matrix *V)
{
    assert(p < m_row && q < m_col);

    Matrix &B = *this;
    int n = m_col;
    int last = n - q - 1;

    // 计算 Wilkinson 位移
    // 注意如果 n-p-q，那么 gamma_{n-2} 设为零
    Real gamma = (n - p - q > 2) ? B(last - 2, last - 1) : 0;
    Real alpha = pow(B(last, last), 2) + pow(B(last - 1, last), 2);
    Real delta = (pow(B(last - 1, last - 1), 2) + gamma * gamma - alpha) / 2;
    Real beta = B(last - 1, last - 1) * B(last - 1, last);
    Real mu = alpha - beta * beta / (delta + SIGN(delta) * sqrt(delta * delta + beta * beta));

    // 计算 Givens 变换的系数，注意第一步 Givens 变换右乘，需要反转变换矩阵
    Vector yz({B(p, p) * B(p, p) - mu, B(p, p) * B(p, p + 1)});
    Vector cs = Givens(yz);
    cs[1] *= -1;

    // 累计正交变换
    if (V != nullptr)
        V->AG(p, p + 1, cs);

    // B * G
    B.AG(p, p + 1, cs);

    // 记录 Givens 变换
    for (int i = p; i < last; i++)
    {
        // 更新 yz
        yz[0] = B(i, i);
        yz[1] = B(i + 1, i);

        // Givens 行变换
        cs = Givens(yz);

        // 累计正交变换
        if (U != nullptr)
            U->AGT(i, i + 1, cs);

        // G * B
        B.GA(i, i + 1, cs);

        // 只要不是最后一行，就获取相邻两列计算
        if (i < last - 1)
        {
            // 更新 yz
            yz[0] = B(i, i + 1);
            yz[1] = B(i, i + 2);

            // Givens 列变换
            cs = Givens(yz);

            // 累计正交变换
            if (V != nullptr)
                V->AG(i + 1, i + 2, cs);

            B.AG(i + 1, i + 2, cs);
        }
    }
}

// 释放资源
Matrix::~Matrix()
{
    for (int j = 0; j < m_row; j++)
        if (m_value[j] != nullptr)
            delete[] m_value[j];
    delete[] m_value;
}

// 构造函数
template <class T>
DynamicArray<T>::DynamicArray() : m_head(nullptr), m_tail(nullptr), m_size(0)
{
}

template <class T>
DynamicArray<T>::DynamicArray(std::initializer_list<T> l) : m_size(l.size())
{
    auto j = l.begin();
    m_head = new Node(*j++, nullptr, nullptr);

    Node *p = m_head;
    for (int i = 1; i < m_size; i++)
    {
        p->next = new Node(*j++, p, nullptr);
        p = p->next;
    }
    m_tail = p;
}

template <class T>
DynamicArray<T>::DynamicArray(const DynamicArray<T> &arr) : m_size(arr.m_size)
{
    m_head = (arr.empty() ? nullptr : new Node(arr[0], nullptr, nullptr));

    Node *p = m_head;
    for (int i = 1; i < m_size; i++)
    {
        p->next = new Node(arr[i], p, nullptr);
        p = p->next;
    }
    m_tail = p;
}

// 拷贝赋值
template <class T>
void DynamicArray<T>::operator=(const DynamicArray<T> &arr)
{
    // 首先清空
    this->clear();

    // 然后依次推入
    Node *node = arr.m_head;
    while (node != nullptr)
    {
        this->push_back(node->value);
        node = node->next;
    }
}

// 获取元素，如果超出数组范围，就创建新的节点达到该范围
template <class T>
T &DynamicArray<T>::operator[](int i)
{
    assert(i >= 0);

    // 如果数组是空的，就先初始化
    if (m_size == 0)
    {
        m_head = new Node(T(), nullptr, nullptr);
        m_tail = m_head;
        m_size++;
    }

    // 如果在范围内，就从头部出发寻找
    Node *p = nullptr;
    if (i < m_size)
    {
        p = m_head;
        while (i > 0)
        {
            p = p->next;
            i--;
        }
    }
    // 如果超出范围，直接从尾部出发
    else
    {
        p = m_tail;
        // 注意要先减去数组范围
        i -= m_size - 1;
        while (i > 0)
        {
            p->next = new Node(T(), p, nullptr);
            p = p->next;
            m_size++;
            i--;
        }
    }

    // 如果这是最后一个节点，那么就作为尾部节点
    if (p->next == nullptr)
        m_tail = p;
    return p->value;
}

// 在数组范围内获取元素
template <class T>
T DynamicArray<T>::operator[](int i) const
{
    assert(i < m_size && i >= 0);
    Node *p = m_head;
    while (i > 0)
    {
        p = p->next;
        i--;
    }
    return p->value;
}

// 获得头部元素
template <class T>
T DynamicArray<T>::front() const
{
    return m_head->value;
}

// 获得尾部元素
template <class T>
T DynamicArray<T>::back() const
{
    return m_tail->value;
}

// 获得指定的节点
template <class T>
typename DynamicArray<T>::Node *DynamicArray<T>::node(int i) const
{
    assert(i < m_size);
    Node *p = m_head;
    while (i > 0)
    {
        p = p->next;
        i--;
    }
    return p;
}

// 在 i 位置插入元素
template <class T>
void DynamicArray<T>::insert(int i, T value)
{
    // 如果超出范围，就直接调用引用修改
    if (i >= m_size)
    {
        operator[](i) = value;
        return;
    }

    Node *q = new Node(value, nullptr, nullptr);
    if (i == 0)
    {
        q->next = m_head;

        if (m_head != nullptr)
        {
            m_head->prev = q;
            m_head = q;
        }
        else
        {
            // 数组是空
            m_head = q;
            m_tail = q;
        }
    }
    else
    {
        // 获取前一个位置的元素和后一个位置
        Node *prev = m_head;
        while (i - 1 > 0)
        {
            i--;
            prev = prev->next;
        }
        Node *next = prev->next;

        // 重新建立连接
        prev->next = q;
        q->prev = prev;
        q->next = next;

        // 如果 q 不是尾部
        if (next != nullptr)
            next->prev = q;
        else
            m_tail = q;
    }
    m_size++;
}

// 在指定节点处插入元素
template <class T>
typename DynamicArray<T>::Node *DynamicArray<T>::insert(Node *node, T value)
{
    Node *q = new Node(value, node->prev, node);

    // 如果是头部节点，就需要修改头部
    if (node == m_head)
        m_head = q;
    else
        node->prev->next = q;

    node->prev = q;
    m_size++;

    return q;
}

// 尾部推入元素
template <class T>
void DynamicArray<T>::push_back(T value)
{
    Node *q = new Node(value, nullptr, nullptr);

    // 如果数组是空，直接作为头尾节点
    if (m_size == 0)
    {
        m_head = q;
        m_tail = q;
    }
    else
    {
        q->prev = m_tail;
        m_tail->next = q;
        m_tail = q;
    }
    m_size++;
}

// 头部推入元素
template <class T>
void DynamicArray<T>::push_front(T value)
{
    Node *q = new Node(value, nullptr, nullptr);

    // 如果数组是空，直接作为头尾节点
    if (m_size == 0)
    {
        m_head = q;
        m_tail = q;
    }
    else
    {
        q->next = m_head;
        m_head->prev = q;
        m_head = q;
    }
    m_size++;
}

// 弹出最后一个元素
template <class T>
T DynamicArray<T>::pop_back()
{
    assert(m_size > 0);
    T value;

    if (m_size == 1)
    {
        value = m_head->value;
        delete m_head;
        m_head = nullptr;
        m_tail = nullptr;
    }
    else
    {
        value = m_tail->value;
        m_tail = m_tail->prev;
        delete m_tail->next;
        m_tail->next = nullptr;
    }
    m_size--;

    return value;
}

// 弹出第一个元素
template <class T>
T DynamicArray<T>::pop_front()
{
    assert(m_size > 0);
    T value;

    if (m_size == 1)
    {
        value = m_head->value;
        delete m_head;
        m_head = nullptr;
        m_tail = nullptr;
    }
    else
    {
        value = m_head->value;
        m_head = m_head->next;
        delete m_head->prev;
        m_head->prev = nullptr;
    }
    m_size--;

    return value;
}

// 找到元素
template <class T>
int DynamicArray<T>::find(T value) const
{
    int index = 0;
    Node *p = m_head;
    while (p != nullptr)
    {
        if (p->value == value)
            return index;
        p = p->next;
        index++;
    }
    return -1;
}

// 删除指定元素
template <class T>
bool DynamicArray<T>::kill(T value)
{
    Node *p = m_head;
    while (p != nullptr)
    {
        if (p->value == value)
        {
            // 头尾的特殊情况
            if (p == m_head)
            {
                this->pop_front();
                return true;
            }
            if (p == m_tail)
            {
                this->pop_back();
                return true;
            }

            // 其余情况正常删除
            Node *prev = p->prev;
            Node *next = p->next;
            prev->next = next;
            next->prev = prev;
            delete p;

            m_size--;
            return true;
        }
        p = p->next;
    }
    return false;
}

// 删除指定位置的元素
template <class T>
T DynamicArray<T>::remove(int i)
{
    assert(i < m_size);

    // 先找到节点
    Node *p = m_head;
    while (i > 0)
    {
        p = p->next;
        i--;
    }

    // 头尾的特殊情况
    if (p == m_head)
        return this->pop_front();
    if (p == m_tail)
        return this->pop_back();

    // 其余情况正常删除
    Node *prev = p->prev;
    Node *next = p->next;
    prev->next = next;
    next->prev = prev;

    T value = p->value;
    delete p;
    m_size--;
    return value;
}

// 移除指定节点的元素
template <class T>
void DynamicArray<T>::remove(Node *node)
{
    T value = node->value;

    // 头尾的特殊情况
    if (node == m_head)
    {
        this->pop_front();
        return;
    }
    if (node == m_tail)
    {
        this->pop_back();
        return;
    }

    // 其余情况正常删除
    Node *prev = node->prev;
    Node *next = node->next;
    prev->next = next;
    next->prev = prev;

    delete node;
    m_size--;
}

// 清空数组
template <class T>
void DynamicArray<T>::clear()
{
    Node *p = m_head;
    Node *q = nullptr;
    while (p != nullptr)
    {
        q = p;
        p = p->next;
        delete q;
    }

    // 初始化成员变量
    m_head = nullptr;
    m_tail = nullptr;
    m_size = 0;
}

// 析构函数
template <class T>
DynamicArray<T>::~DynamicArray()
{
    clear();
}

// 构造函数
SpMatrix::SpMatrix(int Row, int Col) : m_row(Row), m_col(Col)
{
    // 初始非零元都是对角元
    int nnz = MIN(Row, Col);

    // 对角元赋值零
    for (int i = 0; i < Row; i++)
        m_IA.push_back(i);
    for (int i = 0; i < nnz; i++)
    {
        m_JA.push_back(i);
        m_AA.push_back(0);
    }

    // 边界值
    m_JA.push_back(INT_MAX);
    m_IA.push_back(nnz);
    m_AA.push_back(FLT_MAX);
}

// 拷贝构造
SpMatrix::SpMatrix(const SpMatrix &spm) : m_row(spm.m_row), m_col(spm.m_col), m_IA(spm.m_IA), m_JA(spm.m_JA), m_AA(spm.m_AA)
{
}

// 用普通矩阵构造
SpMatrix::SpMatrix(const Matrix &m) : SpMatrix(m.row(), m.col())
{
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            if (fabs(m(i, j)) > ACCURACY)
                this->operator()(i, j) = m(i, j);
}

// 获得行数
int SpMatrix::row() const
{
    return m_row;
}

// 获得列数
int SpMatrix::col() const
{
    return m_col;
}

// 获得非零元个数
int SpMatrix::explicitNum() const
{
    return m_IA.back();
}

// 获取对角元素
Real SpMatrix::diag(int i) const
{
    return m_AA[m_IA[i]];
}

// 获取指定行的开头
int SpMatrix::startOfRow(int i) const
{
    return m_IA[i];
}

// 通过全局指标获取元素所在的列位置
int SpMatrix::indexOfCol(int j) const
{
    return m_JA[j];
}

// 通过全局指标获取元素值
Real SpMatrix::global(int j) const
{
    return m_AA[j];
}

// 获取(i,j)元素的全局列标
int SpMatrix::globalCol(int i, int j) const
{
    DynamicArray<int>::Node *node_IA = m_IA.node(i);
    DynamicArray<int>::Node *node_JA = m_JA.node(node_IA->value);
    for (int k = node_IA->value; k < node_IA->next->value; k++)
    {
        if (node_JA->value == j)
            return k;
        node_JA = node_JA->next;
    }
    return -1;
}

// 重载以获得指定位置的元素，只能获得存在的元素，不存在的元素认为是零
Real SpMatrix::operator()(int i, int j) const
{
    assert(i < m_row && j < m_col);

    DynamicArray<int>::Node *node_IA = m_IA.node(i);
    DynamicArray<int>::Node *node_JA = m_JA.node(node_IA->value);
    for (int k = node_IA->value; k < node_IA->next->value; k++)
    {
        if (node_JA->value == j)
            return m_AA[k];
        node_JA = node_JA->next;
    }

    return 0;
}

// 设置指定位置的元素，不存在就创建新元素的引用返回
Real &SpMatrix::operator()(int i, int j)
{
    assert(i < m_row && j < m_col);

    // 这一行没有实际用处，只是用来返回值，实际上并不会用到
    DynamicArray<Real>::Node *node_AA = m_AA.node(0);

    // 这一行的范围
    DynamicArray<int>::Node *node_IA = m_IA.node(i);
    int start = node_IA->value;
    int end = node_IA->next->value;

    // 如果找到了就结束
    DynamicArray<int>::Node *node_JA = m_JA.node(start);
    DynamicArray<int>::Node *tmp = node_JA;
    for (int k = start; k < end; k++)
    {
        if (tmp->value == j)
            return m_AA[k];
        tmp = tmp->next;
    }

    // 没有找到，要插入一个新的元素，返回它的引用
    node_JA = node_JA->next;
    int index = start + 1;
    for (int k = start; k < end + 1; k++)
    {
        // 遇到第一个大于 j 的位置/这一行只有对角元/到达末尾时插入列标
        if (index == end || node_JA->value > j || k == end)
        {
            // 能够这样插入是因为最后还有一个元素占位，否则动态数组不能插入到最后一个元素位置
            m_JA.insert(node_JA, j);

            // 插入一个零
            node_AA = m_AA.insert(m_AA.node(index), 0);

            // 更新 IA，i 后面每个行标的开头都要后移
            node_IA = node_IA->next;
            for (int p = i + 1; p < m_row + 1; p++)
            {
                node_IA->value++;
                node_IA = node_IA->next;
            }

            return node_AA->value;
        }
        index = k + 1;

        // index == end 和 node_JA->value > j 两种情况插入位置相同
        if (k != start)
            node_JA = node_JA->next;
    }

    return node_AA->value;
}

// 压缩矩阵的零元素，返回移除的零元素数量
SpMatrix SpMatrix::compress() const
{
    SpMatrix spm(m_row, m_col);

    // 第 i 行的起始位置
    DynamicArray<int>::Node *node_IA = m_IA.node(0);
    for (int i = 0; i < m_row; i++)
    {
        int start = node_IA->value;
        int end = node_IA->next->value;

        // 第 i 行的列标开始循环赋值
        DynamicArray<int>::Node *node_JA = m_JA.node(start);
        for (int j = start; j < end; j++)
        {
            Real value = this->operator()(i, node_JA->value);

            // 只有当值足够大才进行赋值
            if (fabs(value) > ACCURACY)
                spm(i, node_JA->value) = value;
            node_JA = node_JA->next;
        }

        node_IA = node_IA->next;
    }

    return spm;
}

// 转置矩阵
SpMatrix SpMatrix::transpose() const
{
    SpMatrix spm(m_row, m_col);

    // 第 i 行的起始位置
    DynamicArray<int>::Node *node_IA = m_IA.node(0);
    for (int i = 0; i < m_row; i++)
    {
        int start = node_IA->value;
        int end = node_IA->next->value;

        // 第 i 行的列标开始循环赋值
        DynamicArray<int>::Node *node_JA = m_JA.node(start);
        for (int j = start; j < end; j++)
        {
            // 反转赋值
            spm(node_JA->value, i) = this->operator()(i, node_JA->value);
            node_JA = node_JA->next;
        }

        node_IA = node_IA->next;
    }

    return spm;
}

// 转化为一般矩阵
Matrix SpMatrix::toMatrix() const
{
    Matrix m(m_row, m_col);
    for (int i = 0; i < m_row; i++)
        for (int j = 0; j < m_col; j++)
            m(i, j) = this->operator()(i, j);
    return m;
}

// 加减法
SpMatrix SpMatrix::operator+(const SpMatrix &spm) const
{
    assert(m_row == spm.m_row && m_col == spm.m_col);

    SpMatrix res = *this;

    // 第 i 行的起始位置
    DynamicArray<int>::Node *node_IA = spm.m_IA.node(0);
    for (int i = 0; i < m_row; i++)
    {
        int start = node_IA->value;
        int end = node_IA->next->value;

        // 第 i 行的列标开始循环赋值
        DynamicArray<int>::Node *node_JA = spm.m_JA.node(start);
        for (int j = start; j < end; j++)
        {
            res(i, node_JA->value) += spm(i, node_JA->value);
            node_JA = node_JA->next;
        }

        node_IA = node_IA->next;
    }

    return res;
}
SpMatrix SpMatrix::operator-(const SpMatrix &spm) const
{
    assert(m_row == spm.m_row && m_col == spm.m_col);

    SpMatrix res = *this;

    // 第 i 行的起始位置
    DynamicArray<int>::Node *node_IA = spm.m_IA.node(0);
    for (int i = 0; i < m_row; i++)
    {
        int start = node_IA->value;
        int end = node_IA->next->value;

        // 第 i 行的列标开始循环赋值
        DynamicArray<int>::Node *node_JA = spm.m_JA.node(start);
        for (int j = start; j < end; j++)
        {
            res(i, node_JA->value) -= spm(i, node_JA->value);
            node_JA = node_JA->next;
        }

        node_IA = node_IA->next;
    }

    return res;
}

// 乘除法运算
SpMatrix SpMatrix::operator*(Real k) const
{
    SpMatrix res = *this;
    DynamicArray<Real>::Node *node_AA = res.m_AA.node(0);

    // 除了最后一个元素以外都乘 k
    for (int i = 0; i < m_AA.size() - 1; i++)
    {
        node_AA->value *= k;
        node_AA = node_AA->next;
    }
    return res;
}
SpMatrix SpMatrix::operator/(Real k) const
{
    assert(fabs(k) > ACCURACY);

    SpMatrix res = *this;
    DynamicArray<Real>::Node *node_AA = res.m_AA.node(0);

    // 除了最后一个元素以外都乘 k
    for (int i = 0; i < m_AA.size() - 1; i++)
    {
        node_AA->value /= k;
        node_AA = node_AA->next;
    }
    return res;
}

// 稀疏矩阵的乘法
SpMatrix SpMatrix::operator*(const SpMatrix &spm) const
{
    assert(m_col == spm.m_col);
    int Col2 = spm.m_col;
    SpMatrix res(m_row, Col2);

    DynamicArray<int>::Node *node_IA = m_IA.node(0);
    for (int i = 0; i < m_row; i++)
    {
        // 暂存数据
        Real *tmp_val = new Real[Col2];
        for (int j = 0; j < Col2; j++)
            tmp_val[j] = 0;

        // 根据 IA 获得 i 行元素中非零的列标索引 IA[i]，对应的列标是 JA[IA[i]]
        DynamicArray<int>::Node *node_JA = m_JA.node(node_IA->value);

        // 计算 i 行的结果
        for (int j = node_IA->value; j < node_IA->next->value; j++)
        {
            // 获得对应 JA[IA[i]] 列的 spm 的 JA[IA[i]] 行元素中非零列标索引
            DynamicArray<int>::Node *spm_node_IA = spm.m_IA.node(node_JA->value);

            // 这是一个很巧妙的计算方式，只需要每次计算两个元素的乘积，记录在对应的列的位置上
            // 然后不断累加即可，相当于将n次乘法和加法分开计算
            for (int k = spm_node_IA->value; k < spm_node_IA->next->value; k++)
                tmp_val[spm.m_JA[k]] += m_AA[j] * spm.m_AA[k];

            node_JA = node_JA->next;
        }

        // 去掉过小的元素
        for (int j = 0; j < Col2; j++)
            if (fabs(tmp_val[j]) > ACCURACY)
                res(i, j) = tmp_val[j];

        delete[] tmp_val;
        node_IA = node_IA->next;
    }
    return res;
}

// 拷贝赋值
void SpMatrix::operator=(const SpMatrix &spm)
{
    assert(m_row == spm.m_row && m_col == spm.m_col);
    m_AA = spm.m_AA;
    m_IA = spm.m_IA;
    m_JA = spm.m_JA;
}

// 向量点积
Real LinearAlgebra::dot(const Vector &vec1, const Vector &vec2)
{
    assert(vec1.size() == vec2.size());
    Real sum = 0;
    for (int i = 0; i < vec1.size(); i++)
        sum += vec1[i] * vec2[i];
    return sum;
}

// Householder 变换
Real LinearAlgebra::Householder(Vector &tx, Vector &v)
{
    // 使用无穷范数规范化防止溢出
    Real eta = tx.norm(0);
    Vector x = tx / eta;
    v = x;
    v[0] = 0;
    Real sigma = dot(x, x) - x[0] * x[0];

    Real beta = 0;
    if (fabs(sigma) > ACCURACY)
    {
        Real alpha = sqrt(x[0] * x[0] + sigma);
        if (x[0] <= 0)
            v[0] = x[0] - alpha;
        // 如果是正的分量，则用特殊算法减小舍入误差
        else
            v[0] = -sigma / (x[0] + alpha);

        // 对 beta 乘 v[0] * v[0] ，抵消规格化为 1 的影响
        beta = 2 * v[0] * v[0] / (sigma + v[0] * v[0]);
        v = v / v[0];
    }
    return beta;
}

// Givens 变换
Vector LinearAlgebra::Givens(Vector &v, int i, int j)
{
    Real sqrtX = sqrt(v[i] * v[i] + v[j] * v[j]);
    if (sqrtX == 0)
        return {1, 0};
    return {v[i] / sqrtX, v[j] / sqrtX};
}

// 希尔伯特矩阵
Matrix LinearAlgebra::Hilbert(int n)
{
    Matrix A(n, n);
    for (int i = 0; i < n; i++)
        for (int j = 0; j < n; j++)
            A(i, j) = 1.0 / (i + j + 1);

    return A;
}

// A 沿着 p,q 分量旋转得到 JT*A*J, Q 累计 J
void LinearAlgebra::rotate(Matrix &A, Matrix &Q, int p, int q)
{
    int n = A.row();
    assert(n == A.col());

    Real cs[2];
    if (A(p, q) == 0)
    {
        cs[0] = 1;
        cs[1] = 0;
    }
    else
    {
        // t*t + 2*tau*t - 1 = 0
        Real tau = (A(q, q) - A(p, p)) / (2 * A(p, q));
        Real t = SIGN(tau);
        t /= (fabs(tau) + sqrt(1 + tau * tau));
        cs[0] = 1 / (sqrt(1 + t * t));
        cs[1] = t * cs[0];
    }
    // JT * A
    for (int j = 0; j < n; j++)
    {
        Real tmp;
        tmp = A(p, j);
        A(p, j) = tmp * cs[0] - A(q, j) * cs[1];
        A(q, j) = cs[1] * tmp + A(q, j) * cs[0];
    }
    // JT * A * J, note: here J is not a symmetical matrix
    // Q * J
    for (int j = 0; j < n; j++)
    {
        Real tmp;
        tmp = A(j, p);
        A(j, p) = tmp * cs[0] - A(j, q) * cs[1];
        A(j, q) = cs[1] * tmp + A(j, q) * cs[0];

        tmp = Q(j, p);
        Q(j, p) = tmp * cs[0] - Q(j, q) * cs[1];
        Q(j, q) = cs[1] * tmp + Q(j, q) * cs[0];
    }
}

// 是否按照单位上三角阵求解
Vector LinearSolver::solveUpper(const Matrix &A, const Vector &b, bool unit)
{
    int n = b.size();
    Vector x(n);
    for (int i = n - 1; i >= 0; i--)
    {
        Real sum = 0;
        for (int j = i + 1; j < n; j++)
            sum += A(i, j) * x[j];
        x[i] = (b[i] - sum) / (unit ? 1 : A(i, i));
    }
    return x;
}

// 是否按照单位下三角阵求解
Vector LinearSolver::solveLower(const Matrix &A, const Vector &b, bool unit)
{
    int n = b.size();
    Vector x(n);
    x[0] = b[0] / (unit ? 1 : A(0, 0));
    for (int i = 1; i < n; i++)
    {
        Real sum = 0;
        for (int j = 0; j < i; j++)
            sum += A(i, j) * x[j];
        x[i] = (b[i] - sum) / (unit ? 1 : A(i, i));
    }
    return x;
}

// 是否选主元求解
Vector LinearSolver::solve(const Matrix &A, const Vector &b, bool choose)
{
    int n = b.size();
    Vector x(n);

    if (choose)
    {
        // 记录置换
        int *perm = new int[n];
        for (int i = 0; i < n; i++)
            perm[i] = i;

        Matrix B = A.LU(perm);

        // 对 b 进行置换
        Vector b1(n);
        for (int i = 0; i < n; i++)
            b1[i] = b[perm[i]];

        x = solveLower(B, b1, true);
        x = solveUpper(B, x);

        delete[] perm;
    }
    else
    {
        Matrix B = A.LU();

        x = solveLower(B, b, true);
        x = solveUpper(B, x);
    }

    return x;
}

// 是否优化为 LDL 分解
Vector LinearSolver::Cholesky(const Matrix &A, const Vector &b, bool improve)
{
    int n = b.size();
    Vector x(n);

    if (improve)
    {
        Matrix B = A.CholeskyD();

        // 将对角元乘到上三角部分
        for (int i = 0; i < n; i++)
            for (int j = i + 1; j < n; j++)
                B(i, j) *= B(i, i);

        x = solveLower(B, b, true);
        x = solveUpper(B, x);
    }
    else
    {
        Matrix B = A.Cholesky();

        x = solveLower(B, b);
        x = solveUpper(B, x);
    }

    return x;
}

// 计算条件数
Real LinearSolver::condition(const Matrix &B)
{
    Matrix A = B;
    int n = A.row();
    assert(n == A.col());

    // 分别分解
    Matrix AT = A.transpose().LU();
    A = A.LU();

    Vector x(n, 1.0 / n), w(n), v(n), z(n);
    while (true)
    {
        // get w
        w = solveLower(AT, x, true);
        w = solveUpper(AT, w);
        v = w.sgn();

        // get z
        z = solveLower(A, v, true);
        z = solveUpper(A, z);

        // compare infinite z and zT * x
        if (z.norm(0) <= dot(z, x))
            break;

        // 将 x 设为 z 最大模位置为 1 的单位向量
        x = Vector(n);
        x[z.maxIndex()] = 1;
    }

    return B.norm() * w.norm(1);
}

// QR 分解法
Vector LinearSolver::QR(const Matrix &A, Vector &b)
{
    int row = A.row();
    int col = A.col();
    Vector d(col);
    Matrix B = A.QR(d);

    // Householder 变换作用于 b
    // Hb = b - beta * v * v^T * b = b - (beta * v^T * b) * v
    for (int i = 0; i < col; i++)
    {
        Real sum = b[i];
        for (int j = i + 1; j < row; j++)
            sum += B(j, i) * b[j];

        sum *= d[i];
        b[i] -= sum;
        for (int j = i + 1; j < row; j++)
            b[j] -= B(j, i) * sum;
    }

    // 求解上半部分的线性方程组
    Vector b1(col);
    for (int i = 0; i < col; i++)
        b1[i] = b[i];

    return solveUpper(B, b1);
}

// 共轭梯度法
Vector LinearSolver::CG(const Matrix &A, const Vector &b)
{
    int n = b.size();
    int count = 0;

    Vector x(n);
    Vector p = b - A * x;
    Vector r = p;

    Real rr = dot(r, r); // get r0 * r0, next r0 * r0

    while (sqrt(rr) > ACCURACY && count < ITERATION)
    {
        Real alpha = rr / dot(p, A * p); // get alpha_0 = r0T * r0 / p0T * A * p0
        x = x + p * alpha;               // get xk = xk-1 + alphak-1 * pk-1
        r = r - A * p * alpha;           // get r = rk-1 - alphak-1 * A * pk-1
        Real nrr = dot(r, r);            // get next r * r, nrr = r * r
        Real beta = nrr / rr;            // get beta = rkT * rk / rk-1T * rk-1
        rr = nrr;                        // get p = rk + betak-1 * pk-1
        p = r + p * beta;                // get alpha
        count++;
    }

    return x;
}

// 幂法计算 A-muI 的最大特征值，然后加回 mu
Real LinearSolver::power(const Matrix &A, Real mu)
{
    int n = A.row();
    assert(n == A.col());

    Vector x(n);
    x[0] = 1;
    Matrix B = A;
    for (int i = 0; i < n; i++)
        B(i, i) -= mu;

    Real dp = 1;
    Real q = 0;

    Vector y(n);
    // 精度要求不能太高，不然算不出来，实际上 1e-5 就可以
    int count = 0;
    while (dp > ACCURACY && count < ITERATION)
    {
        y = B * x;
        // 选择模最大的分量
        Real p = y[y.maxIndex()];
        x = y / p;
        dp = fabs(p - q);
        q = p;
        count++;
    }

    return q + mu;
}

// 反幂法计算指定特征值的特征向量
Vector LinearSolver::inversePower(const Matrix &A, Real lambda)
{
    int n = A.row();
    assert(n == A.col());

    Vector e(n, 1);
    Matrix B = A;
    for (int i = 0; i < n; i++)
        B(i, i) -= lambda;

    // 半次迭代法
    int *p = new int[n];
    B = B.LU(p);
    delete[] p;

    e = solveUpper(B, e);
    return e / e.norm();
}

// Jacobi 迭代法
Vector LinearSolver::Jacobi(const Matrix &A, const Vector &b, Real omega)
{
    int n = A.row();
    assert(A.col() == n);

    // x = D1(L+U) * x + D1 * g
    Vector x(n), y(n);
    Real dx = 0;

    do
    {
        // 分步迭代过程
        for (int i = 0; i < n; i++)
        {
            Real sum = 0;
            for (int j = 0; j < n; j++)
                if (i != j)
                    sum -= A(i, j) * x[j];

            y[i] = (1 - omega) * x[i] + omega * (sum + b[i]) / A(i, i);
        }
        dx = (y - x).norm(0);
        x = y;
    } while (fabs(dx) >= ACCURACY);

    return x;
}

// Gauss-Sediel 迭代
Vector LinearSolver::GaussSediel(const Matrix &A, const Vector &b)
{
    int n = A.row();
    assert(A.col() == n);

    Vector x(n);
    Real dx = 0;

    do
    {
        // 每次刷新误差
        dx = 0;

        // 分部迭代过程
        for (int i = 0; i < n; i++)
        {
            Real sum = 0;
            for (int j = 0; j < n; j++)
                if (j != i)
                    sum -= A(i, j) * x[j];

            Real tmp = x[i];
            x[i] = (sum + b[i]) / A(i, i);

            // 注意这里只记录了一个分量的误差
            dx = MAX(fabs(tmp - x[i]), dx);
        }
    } while (dx >= ACCURACY);

    return x;
}

// 超松弛迭代
Vector LinearSolver::SOR(const Matrix &A, const Vector &b, Real w)
{
    // x = Lw * x + w(D-wL)1 * b
    int n = A.row();
    assert(A.col() == n);

    Vector x(n);
    Real dx = 0;

    do
    {
        // 每次刷新误差
        dx = 0;

        // 分部迭代过程
        for (int i = 0; i < n; i++)
        {
            Real sum = 0;
            for (int j = 0; j < n; j++)
                if (j != i)
                    sum -= A(i, j) * x[j];

            Real tmp = x[i];
            x[i] = (1 - w) * x[i] + w * (sum + b[i]) / A(i, i);

            // 注意这里只记录了一个分量的误差
            dx = MAX(fabs(tmp - x[i]), dx);
        }
    } while (dx >= ACCURACY);

    return x;
}

// 稀疏矩阵的迭代法
Vector LinearSolver::Jacobi(const SpMatrix &A, const Vector &b, Real omega)
{
    int n = A.row();
    assert(A.col() == n);

    Vector x(n), y(n);
    Real dx = 0;

    do
    {
        // 使用节点加速访问
        DynamicArray<int>::Node *node_IA = A.m_IA.node(0);
        for (int i = 0; i < n; i++)
        {
            Real sum = 0;

            DynamicArray<int>::Node *node_JA = A.m_JA.node(node_IA->value + 1);
            DynamicArray<Real>::Node *node_AA = A.m_AA.node(node_IA->value + 1);

            // 只需要计算一次加法 b[i] / d[i,i]
            Real diag = A.global(node_IA->value);
            sum += b[i] / diag;
            for (int j = node_IA->value + 1; j < node_IA->next->value; j++)
            {
                // 累计 D1(L+U) * x
                sum -= node_AA->value * x[node_JA->value] / diag;

                node_JA = node_JA->next;
                node_AA = node_AA->next;
            }
            y[i] = sum;

            node_IA = node_IA->next;
        }
        dx = (y - x).norm(0);
        x = y;
    } while (dx >= ACCURACY);

    return x;
}

Vector LinearSolver::GaussSediel(const SpMatrix &A, const Vector &b)
{
    int n = A.row();
    assert(A.col() == n);

    Vector x(n);
    Real dx = 0;

    do
    {
        // 每次刷新误差
        dx = 0;

        // 使用节点加速访问
        DynamicArray<int>::Node *node_IA = A.m_IA.node(0);
        for (int i = 0; i < n; i++)
        {
            Real sum = 0;

            DynamicArray<int>::Node *node_JA = A.m_JA.node(node_IA->value + 1);
            DynamicArray<Real>::Node *node_AA = A.m_AA.node(node_IA->value + 1);

            // 只需要计算一次加法 b[i] / d[i,i]
            Real diag = A.global(node_IA->value);
            sum += b[i] / diag;
            for (int j = node_IA->value + 1; j < node_IA->next->value; j++)
            {
                // 累计 D1(L+U) * x
                sum -= node_AA->value * x[node_JA->value] / diag;

                node_JA = node_JA->next;
                node_AA = node_AA->next;
            }

            // G_S迭代法每次直接更新计算出的 x[i] 值
            Real tmp = x[i];
            x[i] = sum;

            // 注意这里只记录了一个分量的误差
            dx = MAX(fabs(tmp - x[i]), dx);

            node_IA = node_IA->next;
        }
    } while (dx >= ACCURACY);

    return x;
}

Vector LinearSolver::SOR(const SpMatrix &A, const Vector &b, Real w)
{
    int n = A.row();
    assert(A.col() == n);

    Vector x(n);
    Real dx = 0;

    do
    {
        // 每次刷新误差
        dx = 0;

        // 使用节点加速访问
        DynamicArray<int>::Node *node_IA = A.m_IA.node(0);
        for (int i = 0; i < n; i++)
        {
            Real sum = 0;

            DynamicArray<int>::Node *node_JA = A.m_JA.node(node_IA->value + 1);
            DynamicArray<Real>::Node *node_AA = A.m_AA.node(node_IA->value + 1);

            // 只需要计算一次加法 b[i] / d[i,i]
            Real diag = A.global(node_IA->value);
            sum += b[i] / diag;
            for (int j = node_IA->value + 1; j < node_IA->next->value; j++)
            {
                // 累计 D1(L+U) * x
                sum -= node_AA->value * x[node_JA->value] / diag;

                node_JA = node_JA->next;
                node_AA = node_AA->next;
            }
            // SOR 迭代法每次计算出的 x[i] 值，然后直接乘 w 修正 delta x
            Real tmp = x[i];
            x[i] = (1 - w) * x[i] + w * sum;

            // 注意这里只记录了一个分量的误差
            dx = MAX(fabs(tmp - x[i]), dx);

            node_IA = node_IA->next;
        }
    } while (dx >= ACCURACY);

    return x;
}

// 隐式 QR 方法
Matrix LinearSolver::implicitQR(const Matrix &A, Matrix *Q, bool ifSym)
{
    int l, m, n = A.row(), count = 0;
    assert(n == A.col());

    // 先上 Hessenberg 化
    Matrix B = ifSym ? A.Tridiagonal(Q) : A.Hessenberg(Q);

    // 限制最多迭代 1000 次
    while (count < ITERATION)
    {
        // 将足够小的元素归零
        for (int i = 1; i < n; i++)
        {
            if (fabs(B(i, i - 1)) <= ((fabs(B(i - 1, i - 1)) + fabs(B(i, i))) * ACCURACY))
            {
                B(i, i - 1) = 0;
                ifSym ? B(i - 1, i) = 0 : 0;
            }
        }

        // 划分成三部分
        m = 0;

        if (ifSym)
        {
            // 检查对角阵的范围
            for (int i = n - 1; i > 0; i--)
            {
                // 检查次对角元，如果很大，说明对角阵到这里结束
                if (fabs(B(i, i - 1)) > ACCURACY)
                    break;

                // 已经到达第二行，说明是对角阵，迭代完成
                if (i - 1 == 0)
                    return B;

                m++;
            }
        }
        else
        {
            // 从最后一行向上，检查拟上三角阵的范围，即对角块为 1x1 或 2x2 的矩阵
            Real last = B(n - 1, n - 2);
            for (int i = n - 2; i > 0; i--)
            {
                // 检查两个相邻的次对角元，如果都很大，说明拟三角阵到这里结束
                if (fabs(B(i, i - 1)) > ACCURACY && fabs(last) > ACCURACY)
                    break;

                // 已经到达第二行，说明全是拟上三角，迭代完成，直接返回
                if (i - 1 == 0)
                    return B;

                // 更新次对角
                last = B(i, i - 1);
                m++;
            }
        }

        // 从拟上三角阵头部向上，检查不可约上 Hessenberg 阵的范围
        l = 1;
        for (int i = n - m - 1; i > 0; i--)
            if (fabs(B(i, i - 1)) > ACCURACY)
                l++;

        // 得到上面未约化的矩阵阶数
        l = n - m - l;

        // 对中间的不可约部分进行双重步迭代/Wilkinson 迭代
        if (ifSym)
            B.WilkinsonQR(l, m, Q);
        else
            B.doubleQR(l, m, Q);

        count++;
    }

    return B;
}

// 过关 Jacobi 方法
Matrix LinearSolver::passJacobi(Matrix &A, Real sigma)
{
    int n = A.row();
    assert(n == A.col());

    // Q 初始化为单位阵，用于累计正交变换
    Matrix Q(n, n);
    for (int i = 0; i < n; i++)
        Q(i, i) = 1;

    // delta_k = delta_{k-1} / sigma
    if (sigma < n)
        return Q;

    Real delta = A.norm('e');
    int count = 0;
    while (delta > ACCURACY && count < ITERATION)
    {
        bool check = true;
        for (int i = 0; i < n; i++)
        {
            for (int j = i + 1; j < n; j++)
            {
                if (fabs(A(i, j)) > delta)
                {
                    rotate(A, Q, i, j);
                    check = false;
                }
            }
        }
        // 如果所有分量都小于关值，就减小关值
        if (check)
            delta /= sigma;
        count++;
    }
    return Q;
}

// 二分法，计算三对角对称阵的全部特征值
Vector LinearSolver::bisection(const Matrix &A)
{
    int n = A.row();
    assert(n == A.col());

    // 特征值范围由无穷范数控制
    Real u = A.norm();
    Real l = -1 * u;

    // 初始化系数
    Vector lambda(n), alpha(n), beta(n);
    for (int i = 0; i < n; i++)
    {
        alpha[i] = A(i, i);
        if (i > 0)
            beta[i] = A(i, i - 1);
    }

    // 计算 n 个特征值
    for (int i = 0; i < n; i++)
    {
        Real _u = u;
        Real _l = l;
        Real nowE = u - l;
        Real r = (u + l) / 2;
        while (nowE > ACCURACY)
        {
            // 计算 sn(r)，即 q1(r),q2(r),...,qn(r) 中负数的个数
            int s = 0;
            Real q = alpha[0] - r;
            for (int j = 0; j < n; j++)
            {
                if (q < 0)
                    s++;

                if (j < n - 1)
                {
                    if (q == 0)
                        // replace q with a little positive number
                        q = fabs(beta[j + 1]) * ACCURACY;
                    q = alpha[j + 1] - r - beta[j + 1] * beta[j + 1] / q;
                }
            }
            // 与 i+1 进行比较
            (s >= (i + 1)) ? _u = r : _l = r;
            r = (_u + _l) / 2;
            nowE = _u - _l;
        }
        lambda[i] = r;
    }
    return lambda;
}

// 返回分解后的对角阵，正交阵存放在 U,V 中
Matrix LinearSolver::SVD(const Matrix &A, Matrix *U, Matrix *V)
{
    int p, q, n = A.col(), count = 0;

    // 要求行数不小于列数
    assert(A.row() >= A.col());

    // 先计算上二对角化
    Matrix B = A.Didiagonal(U, V);

    // 限制最多迭代 1000 次
    while (count < ITERATION)
    {
        // 将足够小的元素归零
        for (int i = 0; i < n - 1; i++)
        {
            if (fabs(B(i, i + 1)) <= ((fabs(B(i, i)) + fabs(B(i + 1, i + 1))) * ACCURACY))
                B(i, i + 1) = 0;
        }

        // 把足够小的对角元归零
        for (int i = 0; i < n; i++)
        {
            if (fabs(B(i, i)) <= B.norm() * ACCURACY)
                B(i, i) = 0;
        }

        // 划分成三部分
        q = 0;

        // 检查对角阵的范围
        for (int i = n - 1; i > 0; i--)
        {
            // 检查上次对角元，如果很大，说明对角阵到这里结束
            if (fabs(B(i - 1, i)) > ACCURACY)
                break;

            // 已经到达第二列，说明是对角阵，迭代完成
            if (i - 1 == 0)
                return B;

            q++;
        }

        // 从对角阵头部向上，检查对角元之上的次对角元
        p = 1;
        for (int i = n - q - 1; i > 0; i--)
            if (fabs(B(i - 1, i)) > ACCURACY)
                p++;

        // 得到上面未约化的矩阵阶数
        p = n - q - p;

        // 检查中间不可约部分是否有对角元是零，如果是，那么就将对应行变换归零，然后返回
        // 注意应当倒过来检查，防止出现后面的对角元也是零的情况
        // 不用检查最后一个对角元是零的情况
        bool jump = false;
        for (int i = n - q - 2; i >= p; i--)
        {
            if (B(i, i) == 0)
            {
                // 将 i 行化为零，范围是中间不可约部分
                for (int j = i + 1; j < n - q; j++)
                {
                    // 注意这里 Givens 变换要倒过来算，要将第一个元素化为零
                    Vector x({B(i, j), B(j, j)});
                    Vector cs = Givens(x);
                    Real tmp = cs[0];
                    cs[0] = cs[1];
                    cs[1] = -tmp;

                    // 计算变换
                    B(i, j) = 0;
                    B(j, j) = x.norm();

                    // 如果不是最后一列，就对下一列进行变换
                    if (j < n - q - 1)
                    {
                        B(i, j + 1) = cs[1] * B(j, j + 1);
                        B(j, j + 1) = cs[0] * B(j, j + 1);
                    }

                    // 累计正交变换
                    if (U != nullptr)
                        U->AG(i, j, cs);
                }

                // 重新二对角化
                B = B.Didiagonal(U, V);
                jump = true;
                break;
            }
        }

        // 对中间的不可约部分进行 Wilkinson SVD 迭代
        if (!jump)
            B.WilkinsonSVD(p, q, U, V);

        count++;
    }

    return B;
}